#!/usr/bin/env python
PACKAGE = "mav_linear_mpc"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("q_x",    double_t,    0, "Penality on x axis pos", 50.0 ,  0.001, 200.0)
gen.add("q_y",    double_t,    0, "Penality on y axis pos", 50.0 ,  0.001, 200.0)
gen.add("q_z",    double_t,    0, "Penality on z axis pos", 80.0 ,  0.001, 200.0)

gen.add("q_vx",    double_t,    0, "Penality on x axis vel", 20.0 ,  0.001, 200.0)
gen.add("q_vy",    double_t,    0, "Penality on y axis vel", 20.0 ,  0.001, 200.0)
gen.add("q_vz",    double_t,    0, "Penality on z axis vel", 35.0 ,  0.001, 200.0)

gen.add("q_roll",     double_t,    0, "Penality on roll state", 20.0 ,  0.001, 200.0)
gen.add("q_pitch",    double_t,    0, "Penality on pitch state", 20.0 ,  0.001, 200.0)

gen.add("r_roll",     double_t,    0, "Penality on roll cmd", 30.0 ,  1.0, 200.0)
gen.add("r_pitch",    double_t,    0, "Penality on pitch cmd", 30.0 , 1.0, 200.0)
gen.add("r_thrust",   double_t,    0, "Penality on thrust cmd", 5.0 , 1.0, 200.0)

gen.add("r_droll",     double_t,    0, "Penality on delta roll cmd", 0.26 ,  0.1, 1.0)
gen.add("r_dpitch",    double_t,    0, "Penality on delta pitch cmd", 0.26 ,  0.1, 1.0)
gen.add("r_dthrust",   double_t,    0, "Penality on delta thrust cmd", 0.01 ,  0.001, 0.1)

gen.add("roll_max",     double_t,    0, "max roll cmd [rad]", 0.45, 0.1, 1.0)
gen.add("pitch_max",    double_t,    0, "max pitch cmd [rad]", 0.45, 0.1, 1.0)
gen.add("yaw_rate_max",     double_t,    0, "max yawrate cmd [rad/s]", 1.5, 0.1, 2.0)
gen.add("thrust_min",   double_t,    0, "min thrust cmd [m/s2]", 5.0 ,  5.0, 25.0)
gen.add("thrust_max",   double_t,    0, "max thrust cmd [m/s2]", 15.0 ,  5.0, 25.0)

gen.add("K_yaw",   double_t,    0, "yaw gain", 0.5 ,  0.01, 2.0)
gen.add("Ki_xy",   double_t,    0, "integrator gain for xy", 0.2 ,  0.01, 3.0)
gen.add("Ki_altitude",   double_t,    0, "integrator gain for altitude", 0.2 ,  0.01, 3.0)

gen.add("enable_offset_free",   bool_t,   0, "XY offset free MPC",  True)
gen.add("enable_integrator",   bool_t,   0, "integral action",  False)

exit(gen.generate(PACKAGE, "Config", "LinearMPC"))
